﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Seeker : MonoBehaviour
{
    public enum Mode
    {
        Precision,
        Fast,
        TimeOut
    }

    public Mode mode = Mode.Precision;

    public int TimeOut = 20;
    public int PathLimit = 2;
    public int Bias = 1;

    private void OnDrawGizmos()
    {
        if (mFinalPath == null)
        {
            return;
        }
        if (mFinalPath.Count > 0)
        {
            for (int i = 0; i < mFinalPath.Count - 1; i++)
            {
                var pos1 = mFinalPath[i];
                var pos2 = mFinalPath[i + 1];

                Gizmos.DrawLine(pos1, pos2);
            }
        }
    }

    private readonly PathPoint mFrom = PathPoint.Pop();
    private readonly PathPoint mTo = PathPoint.Pop();

    private Action<bool, List<Vector3>> mCallbackFunc;

    private List<long> mFirstPath = new();
    private List<long> mShortestPath = new();
    private List<PathPoint> mAllPoints = new();
    private List<long> mPath = new();
    private int mPathPointer = 0;
    //private int m_path_cnt = 0;
    private float mShortestDis = float.MaxValue;
    private int mPpCnt = 0;
    private int mJump1Cnt = 0;
    //private int m_jump_2_cnt = 0;

    private readonly Dictionary<long, PathPoint> mDict = new();
    private int mFuncCnt;

    private readonly List<Vector3> mFinalPath = new();

    public void StartPath(Vector3 from, Vector3 to, Action<bool, List<Vector3>> func)
    {
        from.y = 0;
        to.y = 0;

        from = HunterAstarPath.Active.GetNearest(from);
        to = HunterAstarPath.Active.GetNearest(to);

        mFrom.Value = from;
        mTo.Value = to;

        mFinalPath.Clear();

        mCallbackFunc = func;

        StartCoroutine(FindPath());
    }

    private IEnumerator FindPath()
    {
        yield return 0;

        mShortestDis = float.MaxValue;

        mShortestPath.Clear();

        mFuncCnt = 0;
        //m_path_cnt = 0;
        mPpCnt = 0;
        mJump1Cnt = 0;
        //m_jump_2_cnt = 0;

        mFirstPath.Clear();


        System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

        sw.Start();

        //Debug.LogError($"Time={sw.ElapsedMilliseconds} {sw.Elapsed} Start Seek from.Id={m_from.Id} to.Id={m_to.Id}");

        var lst = InitPathPoint();

        //Debug.LogError($"Time={sw.ElapsedMilliseconds} {sw.Elapsed} Sort Dis={Vector3.Distance(m_from.Value, m_to.Value)}");

        mAllPoints.Clear();
        mAllPoints.Add(mFrom);
        mAllPoints.AddRange(lst);

        mDict.Clear();
        foreach (var item in mAllPoints)
        {
            mDict[item.Id] = item;
        }
        mDict[mTo.Id] = mTo;

        mPath.Clear();

        mPath.Add(mFrom.Id);
        mPathPointer = 1;

        var result = CanWalk(mFrom, mTo);
        var isCanWalk = false;
        if (result == false)
        {
            for (int i = 0; i < mAllPoints.Count; i++)
            {
                var item = mAllPoints[i];

                isCanWalk = CanWalk(item, mTo);

                if (isCanWalk)
                {
                    break;
                }
            }

            //if (isCanWalk)
            {
                for (int i = 0; i < mAllPoints.Count; i++)
                {
                    var item = mAllPoints[i];
                    if (mPath.Contains(item.Id))
                    {
                        continue;
                    }

                    Push(item.Id);
                    var r = Func(mFrom, item, sw);
                    Pop();
                    if (r == -1)
                    {
                        break;
                    }
                }
            }
        }
        else
        {
            mPath.Add(mTo.Id);
            mFirstPath.AddRange(mPath);
            mShortestPath.AddRange(mPath);
        }

        sw.Stop();

        //Debug.LogError($"Time = {sw.ElapsedMilliseconds} {sw.Elapsed} PPCnt={m_pp_cnt} Jump1Cnt={m_jump_1_cnt} Jump2Cnt={m_jump_2_cnt}");

        mFinalPath.Clear();
        for (int i = 0; i < mShortestPath.Count; i++)
        {
            var value = mDict[mShortestPath[i]].Value;
            value.y = Core.Utils.Trans.GetGroundHeight(value);
            mFinalPath.Add(value);

        }

        if (result == false && isCanWalk == false)
        {
            Debug.LogError("找不到....................路径");
            //m_final_path.Add(m_from.Value);
            mCallbackFunc?.Invoke(true, mFinalPath);
        }
        else
        {
            mCallbackFunc?.Invoke(false, mFinalPath);
        }

        mDict.Clear();
        mAllPoints.Clear();
    }

    private List<PathPoint> InitPathPoint()
    {
        var lst = new List<PathPoint>();

        //与起始点到目标点的距离之和 3ms -- 10ms Fast 1ms - 2ms
        var dir = mTo.Value - mFrom.Value;
        var rectLst = new List<ObstacleBlock>();
        rectLst.AddRange(HunterAstarPath.Active.Blocks);

        rectLst.Sort((a, b) =>
        {
            var dis1 = Vector3.Distance(a.Center, mFrom.Value) + Vector3.Distance(a.Center, mTo.Value);
            var dis2 = Vector3.Distance(b.Center, mFrom.Value) + Vector3.Distance(b.Center, mTo.Value);

            return dis1.CompareTo(dis2);
        });
        var tempLst = new List<PathPoint>();
        for (int i = 0; i < rectLst.Count; i++)
        {
            tempLst.Clear();
            var pps = rectLst[i].PathPoints;
            for (int j = 0; j < pps.Length; j++)
            {
                if (pps[j].Disable == false)
                {
                    tempLst.Add(pps[j]);
                }
            }
            tempLst.Sort((PathPoint a, PathPoint b) =>
            {
                var dis1 = Vector3.Distance(a.Value, mTo.Value);
                var dis2 = Vector3.Distance(b.Value, mTo.Value);

                return dis1.CompareTo(dis2);
            });
            lst.AddRange(tempLst);
        }

        return lst;
    }

    private void Push(long id)
    {
        mPpCnt++;
        if (mPathPointer >= mPath.Count)
        {
            mPath.Add(id);
        }
        else
        {
            mPath[mPathPointer] = id;
        }
        mPathPointer++;
        //Debug.Log($"Push {string.Join(",", m_path.GetRange(0, m_path_pointer))}");
    }

    private void Pop()
    {
        mPpCnt++;

        mPath[mPathPointer - 1] = 0;
        mPathPointer--;

        //Debug.Log($"Pop {string.Join(",", m_path.GetRange(0, m_path_pointer))}");
    }

    public int Func(PathPoint from, PathPoint to, System.Diagnostics.Stopwatch sw)
    {
        if (mFuncCnt >= PathLimit || TimeOut > 0 && sw.ElapsedMilliseconds > TimeOut)
        {
            return -1;//终止递归
        }

        var dis = CalcDis(mPath, 0, mPathPointer) + Vector3.Distance(to.Value, mTo.Value);
        if (dis >= mShortestDis)
        {
            return 3;//距离过长，不可行走
        }

        var result = CanWalk(from, to);
        if (result == false)
        {
            return 0;//不可以行走
        }

        if (CanWalk(to, mTo))
        {
            Push(mTo.Id);

            dis = CalcDis(mPath, 0, mPathPointer);
            //Debug.Log($" Dis = {dis} PPCnt={m_pp_cnt} Path {string.Join(",", m_path.GetRange(0, m_path_pointer))}");
            if (dis >= mShortestDis)
            {
                Pop();
                return 3;//距离过长，不可行走
            }

            OptimizePath(sw);

            Pop();

            if (Bias > 0 && Mathf.Abs(mShortestDis - Vector3.Distance(mFrom.Value, mTo.Value)) < Bias)
            {
                return -1;
            }

            if (mode == Mode.Fast)
            {
                return -1;
            }

            return 1; //到达终点
        }

        for (int i = 1; i < mAllPoints.Count; i++)
        {
            var item = mAllPoints[i];
            if (mPath.Contains(item.Id))
            {
                continue;
            }

            if (IsDiagnoal(item))
            {
                continue;
            }

            if (mShortestPath.Count > 0)
            {
                var dis1 = CalcDis(mPath, 0, mPathPointer);
                var dis2 = Vector3.Distance(to.Value, mTo.Value);
                if (dis1 + dis2 >= mShortestDis)
                {
                    //Debug.LogError($"Break {string.Join(",", m_path.GetRange(0, m_path_pointer))} item_id={item.Id}");
                    break;
                }

                var idx = mShortestPath.IndexOf(item.Id);
                if (idx >= 0)
                {
                    var dis3 = CalcDis(mShortestPath, 0, idx + 1);
                    var dis4 = Vector3.Distance(to.Value, item.Value);
                    if (dis1 + dis4 > dis3)
                    {
                        //Debug.LogError($"Break 111 {string.Join(",", m_path.GetRange(0, m_path_pointer))} item_id={item.Id}");
                        continue;
                    }
                }

                //var dis1 = CalcDis(m_path, 0, m_path_pointer);
                ////var dis5 = GetDis(to, item) + GetDis(item, m_to);
                var dis5 = Vector3.Distance(to.Value, item.Value) + Vector3.Distance(item.Value, mTo.Value);
                if (dis1 + dis5 > mShortestDis)
                {
                    mJump1Cnt++;
                    continue;
                }
            }

            Push(item.Id);

            var r = Func(to, item, sw);

            Pop();

            if (r == -1)
            {
                return -1;//终止递归
            }
        }


        return 2;//可行走
    }

    public bool CanWalk(PathPoint from, PathPoint to)
    {
        var blocks = HunterAstarPath.Active.Blocks;

        var dis = Vector3.Distance(from.Value, to.Value);
        var center = (from.Value + to.Value) * 0.5f;

        for (int i = 0; i < blocks.Count; i++)
        {
            var rect = blocks[i];
            var d = Vector3.Distance(rect.Center, center);
            if (d > (dis + rect.Length) * 0.5f)
            {
                continue;
            }
            if (Core.Utils.Intersect.IsIntersectLineWithRect(from.Value, to.Value, rect.Points))
            {
                return false;
            }
        }
        return true;
    }

    public bool IsDiagnoal(PathPoint to)
    {
        if (to.DiagonalPoint == null)
        {
            return false;
        }
        for (int i = 0; i < mPathPointer; i++)
        {
            var p = mPath[i];
            var v = mDict[p];
            if (v.DiagonalPoint == null)
            {
                continue;
            }
            if (v.DiagonalPoint == to.DiagonalPoint)
            {
                return true;
            }
        }

        return false;
    }

    public float CalcDis(List<long> path, int idx, int len)
    {
        float dis = 0;
        for (int i = idx; i < idx + len - 1; i++)
        {
            var from = mDict[path[i]];
            var to = mDict[path[i + 1]];
            dis += Vector3.Distance(from.Value, to.Value);
        }
        return dis;
    }


    private void OptimizePath(System.Diagnostics.Stopwatch sw)
    {
        float dis = CalcDis(mPath, 0, mPathPointer);

        bool optimize = false;
        var copyLst = new List<long>();
        copyLst.AddRange(mPath);
        copyLst.RemoveAll(c => c == 0);

        while (true)
        {
            int startIdx = -1;
            int endIdx = -1;
            float maxDis = 0;
            optimize = false;

            for (int i = 0; i < copyLst.Count; i++)
            {
                var p1 = mDict[copyLst[i]];
                for (int j = i + 2; j < copyLst.Count; j++)
                {
                    var p2 = mDict[copyLst[j]];
                    var delta = CalcDis(copyLst, i, j - i + 1) - Vector3.Distance(p1.Value, p2.Value);
                    if (CanWalk(p1, p2) && delta > maxDis)
                    {
                        startIdx = i + 1;
                        endIdx = j;
                        maxDis = delta;
                    }
                }
            }

            if (startIdx >= 0)
            {
                optimize = true;
                //Debug.LogError($"start_idx = {start_idx} end_idx={end_idx} max_dis = {max_dis}");
                //Debug.LogError($"Src Path={string.Join(",", copy_lst)}");

                for (int i = endIdx; i < copyLst.Count; i++)
                {
                    copyLst[startIdx + (i - endIdx)] = copyLst[i];
                }
                for (int i = startIdx + (copyLst.Count - endIdx); i < copyLst.Count; i++)
                {
                    copyLst[i] = 0;
                }

                copyLst.RemoveAll(c => c == 0);
                dis = CalcDis(copyLst, 0, copyLst.Count);

                //Debug.LogError($"Tgt Path={string.Join(",", copy_lst)}");
            }

            if (optimize == false)
            {
                break;
            }
        }


        mFuncCnt++;
        mShortestDis = dis;
        mShortestPath.Clear();

        if (mFirstPath.Count == 0)
        {
            mFirstPath.AddRange(copyLst);
        }
        mShortestPath.AddRange(copyLst);

        //if (Bias > 0 && Mathf.Abs(m_shortest_dis - Vector3.Distance(m_from.Value, m_to.Value)) < Bias)
        //{
        //    Debug.LogError($"Cnt={m_path_cnt++}; Time=={sw.ElapsedMilliseconds} {sw.Elapsed} Dis={dis} PPCnt={m_pp_cnt} JumpCnt={m_jump_1_cnt} [DONE] Path={string.Join(",", m_shortest_path)}");
        //}
        //else
        //{
        //    Debug.LogError($"Cnt={m_path_cnt++}; Time=={sw.ElapsedMilliseconds} {sw.Elapsed} Dis={dis} PPCnt={m_pp_cnt} JumpCnt={m_jump_1_cnt} Path={string.Join(",", m_shortest_path)}");
        //}
    }
}
